/*
 * @ : Copyright (c) 2021 Phytium Information Technology, Inc. 
 *  
 * SPDX-License-Identifier: Apache-2.0.
 * 
 * @Date: 2021-06-29 09:20:24
 * @LastEditTime: 2021-08-16 15:48:09
 * @Description:  This files is for 
 * 
 * @Modify History: 
 *  Ver   Who        Date         Changes
 * ----- ------     --------    --------------------------------------
 */

#include "sdkconfig.h"
#include <stdio.h>
#include "ft_types.h"
#include "mmu.h"
#include "cp15.h"
#include "sdkconfig.h"

/* dump 2nd level page table */
void Dump2ndPageTable(u32 *ptb)
{
    int i;
    int fcnt = 0;

    for (i = 0; i < 256; i++)
    {
        u32 pte2 = ptb[i];
        if ((pte2 & 0x3) == 0)
        {
            if (fcnt == 0)
                printf("    ");
            printf("%04x: ", i);
            fcnt++;
            if (fcnt == 16)
            {
                printf("fault\n");
                fcnt = 0;
            }
            continue;
        }
        if (fcnt != 0)
        {
            printf("fault\n");
            fcnt = 0;
        }

        printf("    %04x: %lx: ", i, pte2);
        if ((pte2 & 0x3) == 0x1)
        {
            printf("L,ap:%lx,xn:%ld,texcb:%02lx\n",
                   ((pte2 >> 7) | (pte2 >> 4)) & 0xf,
                   (pte2 >> 15) & 0x1,
                   ((pte2 >> 10) | (pte2 >> 2)) & 0x1f);
        }
        else
        {
            printf("S,ap:%lx,xn:%ld,texcb:%02lx\n",
                   ((pte2 >> 7) | (pte2 >> 4)) & 0xf, pte2 & 0x1,
                   ((pte2 >> 4) | (pte2 >> 2)) & 0x1f);
        }
    }
}

void DumpPageTable(u32 *ptb)
{
    int i;
    int fcnt = 0;

    printf("page table@%p\n", ptb);
    for (i = 0; i < 1024 * 4; i++)
    {
        u32 pte1 = ptb[i];
        if ((pte1 & 0x3) == 0)
        {
            printf("%03x: ", i);
            fcnt++;
            if (fcnt == 16)
            {
                printf("fault\n");
                fcnt = 0;
            }
            continue;
        }
        if (fcnt != 0)
        {
            printf("fault\n");
            fcnt = 0;
        }

        printf("%03x: %08lx: ", i, pte1);
        if ((pte1 & 0x3) == 0x3)
        {
            printf("LPAE\n");
        }
        else if ((pte1 & 0x3) == 0x1)
        {
            printf("pte,ns:%ld,domain:%ld\n",
                   (pte1 >> 3) & 0x1, (pte1 >> 5) & 0xf);
            /*
             *rt_hw_cpu_dump_page_table_2nd((void*)((pte1 & 0xfffffc000)
             *                               - 0x80000000 + 0xC0000000));
             */
        }
        else if (pte1 & (1 << 18))
        {
            printf("super section,ns:%ld,ap:%lx,xn:%ld,texcb:%02lx\n",
                   (pte1 >> 19) & 0x1,
                   ((pte1 >> 13) | (pte1 >> 10)) & 0xf,
                   (pte1 >> 4) & 0x1,
                   ((pte1 >> 10) | (pte1 >> 2)) & 0x1f);
        }
        else
        {
            printf("section,ns:%ld,ap:%lx,"
                   "xn:%ld,texcb:%02lx,domain:%ld\n",
                   (pte1 >> 19) & 0x1,
                   ((pte1 >> 13) | (pte1 >> 10)) & 0xf,
                   (pte1 >> 4) & 0x1,
                   (((pte1 & (0x7 << 12)) >> 10) |
                    ((pte1 & 0x0c) >> 2)) &
                       0x1f,
                   (pte1 >> 5) & 0xf);
        }
    }
}

/* level1 page table, each entry for 1MB memory. */
static volatile unsigned long MMUTable[4 * 1024] __attribute__((aligned(16 * 1024)));
void SetMMUTable(u32 vaddrStart,
                 u32 vaddrEnd,
                 u32 paddrStart,
                 u32 attr)
{
    volatile u32 *pTT;
    volatile int i, nSec;
    pTT = (u32 *)MMUTable + (vaddrStart >> 20);
    nSec = (vaddrEnd >> 20) - (vaddrStart >> 20);
    for (i = 0; i <= nSec; i++)
    {
        *pTT = attr | (((paddrStart >> 20) + i) << 20);
        pTT++;
    }
}

/* DACR, Defines the access permission for each of the sixteen memory domains. */
unsigned long SetDomainReg(unsigned long domain_val)
{
    unsigned long old_domain;

    asm volatile("mrc p15, 0, %0, c3, c0\n"
                 : "=r"(old_domain));
    asm volatile("mcr p15, 0, %0, c3, c0\n"
                 :
                 : "r"(domain_val)
                 : "memory");

    return old_domain;
}

void InitMMUTable(struct mem_desc *mdesc, u32 size)
{
    /* set page table */
    for (; size > 0; size--)
    {
        SetMMUTable(mdesc->vaddr_start, mdesc->vaddr_end,
                    mdesc->paddr_start, mdesc->attr);
        mdesc++;
    }
}

void InitMMU(void)
{
#ifdef CONFIG_USE_MMU
    cpu_mmu_disable();
    SetDomainReg(0x55555555); //refer to DACR
    //SetDomainReg(0xffffffff);
    cpu_tlb_set(MMUTable);
    cpu_mmu_enable();
#endif
}

void InitCache(void)
{
    cpu_dcache_clean_flush();
    cpu_icache_flush();
    cpu_dcache_disable();
    cpu_icache_disable();
    InitMMU();
    cpu_icache_enable();
    cpu_dcache_enable();
}